function Wind_Server(block)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%
	setup(block);

function setup(block)
	block.NumDialogPrms = 6;

	block.NumInputPorts = 3;
	block.NumOutputPorts = 2;

  block.InputPort(1).Complexity   = 'Real';
  block.InputPort(1).DataTypeId   = 0;
  block.InputPort(1).SamplingMode = 'Sample';
  block.InputPort(1).Dimensions   = 3;

  block.InputPort(2).Complexity   = 'Real';
  block.InputPort(2).DataTypeId   = 0;
  block.InputPort(2).SamplingMode = 'Sample';
  block.InputPort(2).Dimensions   = 1;

  block.InputPort(3).Complexity   = 'Real';
  block.InputPort(3).DataTypeId   = 0;
  block.InputPort(3).SamplingMode = 'Sample';
  block.InputPort(3).Dimensions   = 1;

  block.OutputPort(1).Complexity   = 'Real';
  block.OutputPort(1).DataTypeId   = 0;
  block.OutputPort(1).SamplingMode = 'Sample';
  block.OutputPort(1).Dimensions   = 3;

  block.OutputPort(2).Complexity   = 'Real';
  block.OutputPort(2).DataTypeId   = 0;
  block.OutputPort(2).SamplingMode = 'Sample';
  block.OutputPort(2).Dimensions   = 1;

	block.SampleTimes = [-1 0]; % how do I get varables from the workspace?

	block.RegBlockMethod('PostPropagationSetup',    @DoPostPropSetup);
	block.RegBlockMethod('Start',                   @Start);  
	block.RegBlockMethod('Outputs',                 @Outputs);
	block.RegBlockMethod('Terminate',               @Terminate);

function DoPostPropSetup(block)
	block.NumDworks = 1;

	block.Dwork(1).Name            = 'state';
	block.Dwork(1).Dimensions      = 1;
	block.Dwork(1).DatatypeID      = 0;      % double
	block.Dwork(1).Complexity      = 'Real'; % real
	block.Dwork(1).UsedAsDiscState = true;

function Start(block)
	if block.DialogPrm(4).Data
	  directory = block.DialogPrm(5).Data;
		if isempty(directory)
	    error('Directory name cannnot be blank');
		end
		results = isdir([ directory '/NCR/SIM']);
		if results ~= 1
		  [success, message, messageid] = mkdir(directory);
			if ~ success
				error(messageid, message);
			end
		  mkdir([directory '/NCR']);
		  mkdir([directory '/NCR/SIM']);
		end
		system(['rm ' directory '/NCR/*']);
		system(['rm ' directory '/NCR/SIM/*']);
	end

	block.Dwork(1).Data = 0;
	HandleWeather(block);

function HandleWeather(block)
	persistent start_lat;
	persistent start_lon;
	persistent start_alt;
	persistent start_x;
	persistent start_y;
	persistent start_z;
	persistent weather;

	if isempty(start_lat) || isempty(start_lon)
	  state = 1;
	end

	state = block.Dwork(1).Data;
	switch state
		case 0,
	    fprintf(1,'\n');
	    fprintf(1,'  -------[ Weather Simulator ]-----------------------------\n');
			[weather, start_x, start_y, start_z] = Initialize(block);
			fprintf(1,'  -> Waiting for Connection From HIL Host ');
    case 1,
      [start_lat, start_lon, start_alt] = WaitForClient(block);
			ground = start_alt - start_z;
			lat = start_lat - m_to_lat(start_y);
			lon = start_lon - m_to_lon(start_x,lat);
			weather = setoffset(weather,lat,lon,ground);

			weather_data = sprintf('[%d %d %d %d]', lat, lon, ground, weather.tf - weather.t0);
			set_param('weather_server/Weather Data/weather','value',weather_data);

			fprintf(1,'  -> Handling Requests ...\n');
    case 2,
			SaveRadarImage(block,weather,start_alt,start_z);
			HandleRequest(block, weather, start_lat, start_lon, start_alt, start_x, start_y, start_z);
	end

function SaveRadarImage(block,weather,start_alt,start_z)
  persistent index 

	if (block.Dwork(1).Data == 0) || isempty(index)
	  index = 0;
		return;
	else
		if isempty(index)
			index = 0;
		end
	end

	time = weather.time;

	if index ~= 0
		if (block.InputPort(2).Data) < (time(index) - weather.t0)
			index = 0;
		else
			if size(time,2) < (index + 1)
				return
			end
		end
	end

	next_time = time(index + 1) - weather.t0;

	if block.InputPort(2).Data > next_time
		alt       = block.InputPort(1).Data(3);
		ground = start_alt - start_z;
		z = alt-ground;

		fprintf(1,'saving image for index %d\n',index+1);

	  directory = [block.DialogPrm(5).Data '/NCR/'];

	  img_file = [directory 'SIM_NCR_0.gif'];
		plot(weather,next_time+weather.t0,z,'radar image',img_file);

	  gfw_file = [directory 'SIM_NCR_0.gfw'];
		writegfw(weather,next_time+weather.t0,gfw_file);

		index = index+1;
	end

function [weather, start_x, start_y, start_z] = Initialize(block)
	settings.datadir = '../../data/';

	switch block.DialogPrm(1).Data
		case 1, settings.dataset='2007-08-09';
		case 2, settings.dataset='2007-10-25';
		case 3, settings.dataset='2008-06-06';
		case 4, settings.dataset='2008-06-13';
		case 5, settings.dataset='2009-10-26';
	end

	weather = Weather(settings);

	if block.DialogPrm(2).Data
		plot(weather,weather.t0,weather.z0,'rain wind',1);
		fprintf(1,'  -> Select UA Start Position ');
		[start_x start_y] = ginput(1);
		start_z = 300;
		start_pos = sprintf('[%d %d %d]', start_x, start_y, start_z);
		set_param('weather_server/Weather Data','ua_start_pos',start_pos);
		fprintf(1,'                    [ OK ]\n');
	else 
		start_x = block.DialogPrm(3).Data(1);
		start_y = block.DialogPrm(3).Data(2);
		start_z = block.DialogPrm(3).Data(3);
	end

	block.Dwork(1).Data = block.Dwork(1).Data + 1;

function [start_lat, start_lon, start_alt] = WaitForClient(block)
	time = block.InputPort(2).Data;
	start_lat = []; start_lon = []; start_alt = [];

	if time >= 0
		start_lat = block.InputPort(1).Data(1);
		start_lon = block.InputPort(1).Data(2);
		start_alt = block.InputPort(1).Data(3);

		start_gps = sprintf('[%d %d %d]', start_lat, start_lon, start_alt);
		set_param('weather_server/Weather Data/ua_start_gps','value',start_gps);

		block.Dwork(1).Data = block.Dwork(1).Data + 1;
		fprintf(1,'        [ OK ]\n');
	end

function HandleRequest(block, weather, start_lat, start_lon, start_alt, start_x, start_y, start_z)
	lat       = block.InputPort(1).Data(1);
	lon       = block.InputPort(1).Data(2);
	alt       = block.InputPort(1).Data(3);
	time      = block.InputPort(2).Data;
	hdg       = block.InputPort(3).Data;

	p1.lat = 0;          p1.lon = start_lon;
	p2.lat = 0;          p2.lon = lon;

	diff_x = Vincenty(p1,p2,1);
	if p2.lon < p1.lon
		diff_x = diff_x * -1;
	end
	x = diff_x + start_x;

	p1.lat = start_lat;  p1.lon = 0;
	p2.lat = lat;        p2.lon = 0;
	diff_y = Vincenty(p1,p2,1);
	if p2.lat < p1.lat
		diff_y = diff_y * -1;
	end
	y = diff_y + start_y;

	if isnan(y)
	  y = start_y;
	end

	if isnan(x)
	  x = start_x;
	end

	ground = start_alt - start_z;  %[m]  start 300 m up
	z = alt-ground;

	%current_pos = sprintf('[%d %d %d]', x, y, z);
	current_pos = sprintf('[%d %d %d]', diff_x, diff_y, z);
	set_param('weather_server/Weather Data/ua_current_pos','value',current_pos);

	if block.DialogPrm(6).Data
		aircraft = Aircraft();
		aircraft.position = [x,y,z];
		aircraft.euler = [0,0,hdg*pi/180];

		plot(weather,time+weather.t0,z,'z wind il',1);
		axis([x-15000 x+15000 y-15000 y+15000]);
		plot(aircraft,1);
		axis([x-15000 x+15000 y-15000 y+15000]);

%		if(mod(floor(time),400)) == 0
%			%plot(weather,time+weather.t0,z,'rain wind journal',floor(time)/400+1);
%			plot(weather,time+weather.t0,z,'z wind',floor(time)/400+1);
%			axis([x-15000 x+15000 y-15000 y+15000]);
%			%hold on;
%			%plotFP('mnr2.fp');
%			%plotFP('tempest2.fp');
%			plot(aircraft,floor(time)/400+1);
%			axis([x-15000 x+15000 y-15000 y+15000]);
%		end
%		if floor(time) == 1
%			%plot(weather,time+weather.t0,z,'rain wind journal',1);
%			plot(weather,time+weather.t0,z,'z wind',1);
%			axis([x-15000 x+15000 y-15000 y+15000]);
%			%hold on;
%			%plotFP('mnr2.fp');
%			%plotFP('tempest2.fp');
%			plot(aircraft,1);
%			axis([x-15000 x+15000 y-15000 y+15000]);
%		end
	end

  vector = weather(time+weather.t0, [x y alt-ground]);
  u = vector(1);
  v = vector(2);
  w = vector(3);
  rain = vector(4);
  hail = vector(5);
	
	%u = 0; v = 0; w = 0;

	if isnan(u)
	  u = 0;
	end
	if isnan(v)
	  v = 0;
	end
	if isnan(w)
	  w = 0;
	end

	block.OutputPort(1).Data(1) = v;
	block.OutputPort(1).Data(2) = u;
	block.OutputPort(1).Data(3) = w;

	% stop simulation after weather data is done
	if (weather.t0 + time) > weather.tf
		block.OutputPort(2).Data = 1;
	else
		block.OutputPort(2).Data = 0;
	end

function Outputs(block)
	HandleWeather(block);

function Terminate(block)
